#include "stdint.h"
#include "Clock.h"
#include "xunji.h"
#include "Motor.h"
#include "msp.h"
#include <stdio.h>
#include <string.h>
#include "Reflectance.h"

int left_wei;
int right_wei;
uint8_t data;
uint8_t led_status;

void cal_weight()
{
    left_wei = right_wei = 0;
    for (int i=0 ; i<4 ; ++i) if (data & (1<<i)) {
        left_wei += (4-i);
    }
    for (int i=0 ; i<4 ; ++i) if (data & (0x80>>i)) {
        right_wei += (4-i);
    }
}

static void LED_init()
{
    P1->OUT &= ~0x07;
    P1->OUT |= 0x01;
    P2->OUT &= ~0x04;
    P2->OUT |= 0x00;
}

static void LED_flip()
{
    P1->OUT ^= 0x01;
    P2->OUT ^= 0x04;
}

static inline int abs(int a)
{
    return a>=0 ? a : -a;
}

#define FAST 6000
#define SLOW 6500

void xunji(uint8_t d)
{
    LED_init();
    while (1) {
        data = Reflectance_Read(500);
        cal_weight();
        if (left_wei == right_wei) {
            if (left_wei>=9) {
                // T crossing
                Motor_Left(SLOW, FAST);
                Clock_Delay1ms(200);
            } else if (left_wei==0) {
                // turn over
                Motor_Left(SLOW, FAST);
            } else {
                Motor_Forward(FAST, FAST);
            }
        } else if (abs(left_wei-right_wei)<=3) {
            Motor_Forward(FAST, FAST);
        } else if (left_wei < right_wei) {
            Motor_Right(SLOW, FAST);
        } else {
            Motor_Left(FAST, SLOW);
        }
        LED_flip();
        Clock_Delay1ms(200);
    }
}
